#! /usr/bin/env python
import rospy
from gazebo_msgs.msg import ModelState
from geometry_msgs.msg import PoseStamped

class pose_publisher:
    def __init__(self):
        rospy.init_node("pose_publisher")
        self.uav_num = 3
        self.local_pose = [PoseStamped() for i in range(self.uav_num)]
        # subscribers
        self.local_pose_sub_0 = rospy.Subscriber("/UAV0/mavros/local_position/pose", PoseStamped,
                                                 self.local_pose_callback, 0)
        self.local_pose_sub_1 = rospy.Subscriber("/UAV1/mavros/local_position/pose", PoseStamped,
                                                 self.local_pose_callback, 1)
        self.local_pose_sub_2 = rospy.Subscriber("/UAV2/mavros/local_position/pose", PoseStamped,
                                                 self.local_pose_callback, 2)
        # publishers
        self.pose_pub = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=10)

    def pose_publisher(self,id):
        pose_msg = ModelState()
        pose_msg.model_name = 'real_iris_'+str(id)
        pose_msg.pose.position.x = self.local_pose[id].pose.position.x
        pose_msg.pose.position.y = self.local_pose[id].pose.position.y
        pose_msg.pose.position.z = self.local_pose[id].pose.position.z+0.38
        pose_msg.pose.orientation.x = self.local_pose[id].pose.orientation.x
        pose_msg.pose.orientation.y = self.local_pose[id].pose.orientation.y
        pose_msg.pose.orientation.z = self.local_pose[id].pose.orientation.z
        pose_msg.pose.orientation.w = self.local_pose[id].pose.orientation.w
        self.pose_pub.publish(pose_msg)

    def local_pose_callback(self, msg, id):
        self.local_pose[id] = msg

if __name__ == "__main__":
    map = pose_publisher()
    rate = rospy.Rate(30)
    while not rospy.is_shutdown():
        try:
            for id in range(map.uav_num):
                map.pose_publisher(id)
        except rospy.ROSInterruptException:
            pass
        rate.sleep()